
#include "stm32f10x.h"
#include <stdlib.h>
#include "MyProject.h"
#include "adc.h"

/************************************************
   
	 
	 
	 
=================================================


************************************************/
/******************************************************************************/
#define LED_blink    GPIOC->ODR^=(1<<13)
/******************************************************************************/
float target;//目标值，现在给的是q轴电压
unsigned int T=50;//周期ms
float M=0.2;//调制比0~1
DQCurrent_s current_cal;
/******************************************************************************/
void commander_run(void);
/******************************************************************************/
void MOTOR_PWM_OFF(void)
{
	TIM_SetCompare1(TIM2,0*PWM_Period);
	TIM_SetCompare2(TIM2,0*PWM_Period);
	TIM_SetCompare3(TIM2,0*PWM_Period);
}
	
void GPIO_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC|RCC_APB2Periph_AFIO, ENABLE);//使能GPIOA,GPIOB,GPIOC,AFIO;
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;         //PC13是LED
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;   //推挽输出	
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;  //速度
	GPIO_Init(GPIOC, &GPIO_InitStructure);             //对选中管脚初始化
	GPIO_SetBits(GPIOC,GPIO_Pin_13);                   //上电点亮LED
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;          //PA6是电机使能
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	GPIO_SetBits(GPIOA,GPIO_Pin_6);                  //低电平解除,Motor_init()中使能
}

float T0,T1,T2;
float Ta,Tb,Tc;



void setPhaseVoltage_my(float Uq, float Ud, float angle_el)
{
	float Uout;
	uint32_t sector;
	
	

	if(Ud) // only if Ud and Uq set 
	{// _sqrt is an approx of sqrt (3-4% error)
		Uout = _sqrt(Ud*Ud + Uq*Uq) / voltage_power_supply;
		// angle normalisation in between 0 and 2pi
		// only necessary if using _sin and _cos - approximation functions
		angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
	}
	///////////////////////////////////////////////////////Ud=0，控制Uq
	else
	{// only Uq available - no need for atan2 and sqrt
		Uout = Uq / voltage_power_supply;
		// angle normalisation in between 0 and 2pi
		// only necessary if using _sin and _cos - approximation functions
		angle_el = _normalizeAngle(angle_el + _PI_2);//只要求产生垂直于当前角度的目标方向
	}
	///////////////////////////////////////////////////////
	if(Uout> 0.2)Uout= 0.2;
	if(Uout<-0.2)Uout=-0.2;
	
	sector = (angle_el / _PI_3) + 1;
	T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout;
	T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uout;
	T0 = 1 - T1 - T2;
	
	// calculate the duty cycles(times)
	switch(sector)
	{
		case 1:
			Ta = T1 + T2 + T0/2;
			Tb = T2 + T0/2;
			Tc = T0/2;
			break;
		case 2:
			Ta = T1 +  T0/2;
			Tb = T1 + T2 + T0/2;
			Tc = T0/2;
			break;
		case 3:
			Ta = T0/2;
			Tb = T1 + T2 + T0/2;
			Tc = T2 + T0/2;
			break;
		case 4:
			Ta = T0/2;
			Tb = T1+ T0/2;
			Tc = T1 + T2 + T0/2;
			break;
		case 5:
			Ta = T2 + T0/2;
			Tb = T0/2;
			Tc = T1 + T2 + T0/2;
			break;
		case 6:
			Ta = T1 + T2 + T0/2;
			Tb = T0/2;
			Tc = T1 + T0/2;
			break;
		default:  // possible error state
			Ta = 0;
			Tb = 0;
			Tc = 0;
	}
	
	TIM_SetCompare1(TIM2,Ta*PWM_Period);
	TIM_SetCompare2(TIM2,Tb*PWM_Period);
	TIM_SetCompare3(TIM2,Tc*PWM_Period);
}
//PID参数：
float pid_current_P_my=0.005;
float pid_current_I_my=0.05;
float integral_current_prev_my=0;
float proportional,integral,out;

float pid_current_P_my_d=0.1;
float pid_current_I_my_d=0.01;
float integral_current_prev_my_d=0;
float proportional_d,integral_d,out_d;

float PID_current_my(float error){
	
	proportional = pid_current_P_my * error;
	integral = integral_current_prev_my + error;
	
	if(integral*pid_current_I_my>5) integral=5/pid_current_I_my;
	if(integral*pid_current_I_my<-5) integral=-5/pid_current_I_my;
	integral_current_prev_my=integral;
	
	out=proportional+pid_current_I_my*integral;
	return  out;

}
float PID_current_my_d(float error){
	
	proportional_d = pid_current_P_my_d * error;
	integral_d = integral_current_prev_my_d + error;
	
	if(integral_d*pid_current_I_my_d>5) integral_d=5/pid_current_I_my_d;
	if(integral_d*pid_current_I_my_d<-5) integral_d=-5/pid_current_I_my_d;
	integral_current_prev_my_d=integral_d;
	
	out_d=proportional_d+pid_current_I_my_d*integral_d;
	return  out_d;

}


int main(void)
{
	unsigned int count_i=0;
	//float Ta,Tb,Tc;
	float Angle_last=0;
	float Angle_now=0;
	unsigned short int adcx1,adcx2;
	float Ua,Ub;	
	float dir=0;
	float angle_target=0;
	float angle_nor=0;
	unsigned int ShanQu=0;
	
	
	GPIO_Config();//初始化PC13是LED、PC13是LE
	uart_init(115200);//串口初始化，串口1初始化，PA9和PA10
	
	I2C_Init_();               //AS5600，PB6和PB7
	printf("AS5600\r\n");

	TIM2_PWM_Init();//PWM高级定时器初始化，PA0\PA1\PA2

	delay_ms(100);
	
	MagneticSensor_Init();     //AS5600 or TLE5012B，磁编码器初始化
	
	Adc_Init();		  		//ADC初始化
	
	voltage_power_supply=10;   //V，需要根据这个值给定Uq,Ud?
	voltage_limit=4;           //V，最大值需小于12/1.732=6.9,即电压的有效值极值
	velocity_limit=10;         //rad/s angleOpenloop() and PID_angle() use it
	//校准电机需要的Uq
	voltage_sensor_align=1;    //V     alignSensor() and driverAlign() use it，大功率电机0.5-1，小功率电机2-3，
	torque_controller=Type_voltage;  //当前只有电压模式，Type_voltage=0
	controller=Type_angle;  //Type_angle; 先用角度控制使的角度启动初始化
	target=0;
	
	pole_pairs=11;
	Motor_init();//打开PB9使能，voltage_sensor_align还是3
	Motor_initFOC();//电机启动初始化
	PID_init();//PID参数初始化
    //printf("Motor ready.\r\n");
	MOTOR_PWM_OFF();//使用关闭PWM来停转电机，可以省电0.1W
	shaft_velocity_sp = 0;
	angle_target = shaftAngle();// shaft angle;
	
	TIM3_1ms_Init();           //interrupt per 1ms,
	
	systick_CountMode();   //不能再调用delay_us()和delay_ms()函数
	shaft_angle_sp=0;
	while(1)
	{
		count_i++;
		Angle_now=getAngle();

		if(time1_cntr>=1000)  //1000ms，在TIM3_IRQHandler中++,1000*1ms=1000ms
		{
			//printf("%.4f,%.4f,%.4f,%.2f,%.2f,%.2f\r\n",angle_target,shaft_angle_sp,shaft_angle,
			//shaft_velocity,Ua,Ub);
			shaft_angle_sp=shaft_angle_sp+2*3.14/36;
			time1_cntr=0;		
		}
		if(time2_cntr>=100)//，100ms在TIM3_IRQHandler中++,
		{
			LED_blink;
			time2_cntr=0;
			//printf("%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\r\n",current_ADC.a,current_ADC.b,current_cal.q,current_cal.d,0.1 - current_cal.q,voltage.q,voltage.d,
			//proportional,integral,out);
		}
					
		//位置环********************************
		//shaft_angle_sp=2;//在这里设置
		shaft_angle = shaftAngle();

		// calculate the torque command
		current_sp = PID_angle(shaft_angle_sp - shaft_angle); // if current/foc_current torque control
		// if torque controlled through voltage control 
		voltage.q = current_sp;  // use voltage if phase-resistance not provided
		voltage.d = 0;

		electrical_angle = electricalAngle();// electrical angle - need shaftAngle to be called first
		setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
		//位置环
	}
}
/******************************************************************************/



